#include "nrf_task.h"
#include "motor.h"

int inits_flag=0,init_flag=0;

void u8_to_float(uint8_t* datain,float* dataout)
{
    *dataout=*(float *)datain;
}

void Nrf_task(void const * argument)
{
    while(NRF24L01_Check())
	{
		osDelay(100);
	}
    nrf_found_flag=1;
	osDelay(100);
	NRF24L01_RX_Mode();
    while(1){
        if(NRF24L01_RxPacket(recv)==0)
        {
            for(int i=0;i<4;i++){
                pitch_u8[i]=recv[i];
            }
            for(int i=0;i<4;i++){
                yaw_u8[i]=recv[i+4];
            }
            for(int i=0;i<4;i++){
                roll_u8[i]=recv[i+8];
            }
            u8_to_float(pitch_u8,&pitch);
            u8_to_float(roll_u8,&roll);
            u8_to_float(yaw_u8,&yaw);
        }
        if(inits_flag==0 && yaw!=0.0f){
            motor_data[0].endc=((-yaw)/180.0f)*580.0f;
            inits_flag=1;
        }
        if(init_flag==0 && roll!=0.0f){
            motor_data[1].endc=((-roll)/180.0f)*580.0f;
            init_flag=1;
        }
		angle_tar[0]=-yaw;
        angle_tar[1]=-roll;
        osDelay(1);
    }
}
